#include "ueq_fifo.h"

#include <stdlib.h>

static void ueq_fifo_increment_start(UeqFifo *pcb, INT32 incr);
static void ueq_fifo_increment_end(UeqFifo *pcb, INT32 incr);

void ueq_fifo_init(UeqFifo *pcb, BYTE *pb, INT32 size)
{
    UEQ_ASSERT(pcb != NULL);
    UEQ_ASSERT(pb != NULL);
    UEQ_ASSERT(size > 1);

    pcb->size   = size;
    pcb->start  = 0;
    pcb->end    = 0;
    pcb->buffer = pb;
    pcb->size_is_pow_of_2 = UEQ_IS_POW_OF_2(size);
}

void ueq_fifo_reset(UeqFifo *pcb)
{
    UEQ_ASSERT(pcb != NULL);

    pcb->size   = 0;
    pcb->start  = 0;
    pcb->end    = 0;
    pcb->buffer = NULL;
    pcb->size_is_pow_of_2 = FALSE;
}

BOOL ueq_fifo_is_full(UeqFifo *pcb)
{
    BOOL result = FALSE;
    UEQ_ASSERT(pcb != NULL);
    /* (end + 1) % size == start */

    if (pcb->size_is_pow_of_2)
    {
        result = (BOOL)(((pcb->end + 1) & (pcb->size - 1)) == pcb->start);
    }
    else
    {
        if (pcb->end + 1 > pcb->size - 1)
        {
            /* [xxxx ]
               [S   E] */
            result = (BOOL)((pcb->end + 1 - pcb->size) == pcb->start);
        }
        else
        {
            /* [xx xx]
               [  ES ] */
            result = (BOOL)((pcb->end + 1) == pcb->start);
        }
    }

    return result;
}

BOOL ueq_fifo_is_empty(UeqFifo *pcb)
{
    UEQ_ASSERT(pcb != NULL);
    return (BOOL)(pcb->end == pcb->start);
}

/* User needs to check if the circular buffer is full first */
void ueq_fifo_push_back(UeqFifo *pcb, BYTE data)
{
    UEQ_ASSERT(pcb != NULL);
    pcb->buffer[pcb->end] = data;
    ueq_fifo_increment_end(pcb, 1);
}

/* User needs to check if the circular buffer is empty first */
BYTE ueq_fifo_pop_front(UeqFifo *pcb)
{
    UEQ_ASSERT(pcb != NULL);
    BYTE data = pcb->buffer[pcb->start];
    ueq_fifo_increment_start(pcb, 1);
    return data;
}

BOOL ueq_fifo_write(UeqFifo *pcb, BYTE *data, INT32 data_size)
{
    UEQ_ASSERT(pcb != NULL);
    UEQ_ASSERT(data != NULL);
    UEQ_ASSERT(data_size > 0);

    if (data_size > pcb->size - 1 - abs(pcb->end - pcb->start))
    {
        return FALSE;
    }

    if (pcb->end < pcb->start)
    {
        memcpy(&pcb->buffer[pcb->end], data, data_size);
    }
    else
    {
        INT32 first_part = pcb->size - pcb->end;

        if (data_size <= first_part)
        {
            memcpy(&pcb->buffer[pcb->end], data, data_size);
        }
        else
        {
            memcpy(&pcb->buffer[pcb->end], data, first_part);
            memcpy(&pcb->buffer[0], &data[first_part], data_size - first_part);
        }
    }

    ueq_fifo_increment_end(pcb, data_size);

    return TRUE;
}

BOOL ueq_fifo_read(UeqFifo *pcb, BYTE *data, INT32 data_size)
{
    UEQ_ASSERT(pcb != NULL);
    UEQ_ASSERT(data != NULL);
    UEQ_ASSERT(data_size > 0);

    //[..12..]
    //[2....1]
    if (pcb->end > pcb->start)
    {
        if (data_size > pcb->end - pcb->start)
        {
            return FALSE;
        }
    }
    else
    {
        if (data_size > pcb->end + pcb->size - pcb->start)
        {
            return FALSE;
        }
    }

    if (pcb->end > pcb->start)
    {
        memcpy(data, &pcb->buffer[pcb->start], data_size);
    }
    else
    {
        INT32 first_part = pcb->size - pcb->start;

        if (data_size <= first_part)
        {
            memcpy(data, &pcb->buffer[pcb->start], data_size);
        }
        else
        {
            memcpy(data, &pcb->buffer[pcb->start], first_part);
            memcpy( &data[first_part], &pcb->buffer[0], data_size - first_part);
        }
    }

    ueq_fifo_increment_start(pcb, data_size);

    return TRUE;
}

static void ueq_fifo_increment_start(UeqFifo *pcb, INT32 incr)
{
    if (pcb->size_is_pow_of_2)
    {
        pcb->start = (pcb->start + incr) & (pcb->size - 1);
    }
    else
    {
        if (pcb->start + incr > pcb->size - 1)
        {
            pcb->start = pcb->start + incr - pcb->size;
        }
        else
        {
            pcb->start = pcb->start + incr;
        }
    }
}

static void ueq_fifo_increment_end(UeqFifo *pcb, INT32 incr)
{
    if (pcb->size_is_pow_of_2)
    {
        pcb->end = (pcb->end + incr) & (pcb->size - 1);
    }
    else
    {
        if (pcb->end + incr > pcb->size - 1)
        {
            pcb->end = pcb->end + incr - pcb->size;
        }
        else
        {
            pcb->end = pcb->end + incr;
        }
    }
}
